#include "Communication/SubMarginSensor.h"
#include "modules/CtrlModule/ActivePlanner/ObsFollowPlanner.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
MarginSensorData DataSet::margin_data;
namespace SubMarginSensor
{

static rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr marginSub;

/*void sub_callback(const std_msgs::msg::Float32::SharedPtr msg)
{
    std::cout << "SubMarginSensor sub_callback" << std::endl;
    DataSet::margin_data.SetDis(msg->data);
    obs_follow_planner.UpdateObsDistance(msg->data);
    DataSet::margin_data.SetTime();
}*/
void sub_callback(const sensor_msgs::msg::Range::SharedPtr msg)
{
    DataSet::margin_data.SetDis(msg->range);
    obs_follow_planner.UpdateObsDistance(msg->range);
    DataSet::margin_data.SetTime();
}

void SubMarginSensorInfo(rclcpp::Node::SharedPtr &node_handle)
{
    marginSub = node_handle->create_subscription<sensor_msgs::msg::Range>(
            "margin_sensor", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubMarginSensor SubMarginSensorInfo" << std::endl;
}
} // namespace SubMarginSensor

} // namespace behavior_controller